import numpy as np
import torch
import open3d as o3d
R_to_r=np.array([[ 0.0720988,  -0.08446065, -0.99381495],
     [-0.02712438,  0.99587354 ,-0.08660341],
     [ 0.9970286 ,  0.03320062,  0.06951035]])
T_to_r=np.array([[0.19801615],
     [0.02060221],
     [0.17645528]])
R_to_l=np.array([[ 0.74910648 , 0.02354084 , 0.66203121],
     [-0.029508 ,   0.99956222 ,-0.00215384],
     [-0.66179209, -0.01792176,  0.74947318]])
T_to_l=np.array([[-0.15883266],
     [-0.00363137],
     [ 0.04947065]])
def show_mult_face(transformed_points,trans_left,trans_right):
    # pcd_left蓝
    pcd_left = o3d.geometry.PointCloud()
    pcd_left.points = o3d.utility.Vector3dVector(trans_left)
    pcd_left.paint_uniform_color([0, 0, 255])

    # pcd_right绿
    pcd_right = o3d.geometry.PointCloud()
    pcd_right.points = o3d.utility.Vector3dVector(trans_right)
    pcd_right.paint_uniform_color([0, 255, 0])

    # pcd_mid黄
    pcd_mid = o3d.geometry.PointCloud()
    pcd_mid.points = o3d.utility.Vector3dVector(transformed_points)
    pcd_mid.paint_uniform_color([255, 255, 0])
    o3d.visualization.draw_geometries([pcd_mid, pcd_left, pcd_right])
def transform_points_to_side_view(P_mid, R, T):
    """
    将 mid 视角下的 3D 点云变换到 side 视角下
    :param P_mid: mid 视角下的 3D 点云，形状为 (N, 3)
    :param R: 旋转矩阵，形状为 (3, 3)
    :param T: 平移向量，形状为 (3, 1)
    :return: side 视角下的 3D 点云，形状为 (N, 3)
    """
    # 将 P_mid 转换为列向量（形状为 (3, N)）
    P_mid = P_mid.T  # 形状为 (3, N)

    # 应用旋转和平移
    P_side = R @ P_mid + T  # 形状为 (3, N)

    # 转置回 (N, 3)
    P_side = P_side.T

    return P_side
def getpoints():
    file = "../data/1005/face3d.txt"
    #file="E:/codes/3DFace/BallFace3D/Data/1001/face3d_old.txt"
    leftPoints=[]
    midPoints=[]
    rightPoint=[]
    with open(file, 'r', encoding='utf-8') as f:
        for idx, line in enumerate(f):
            tmpData = []
            for dt in line.replace("\n", "").split(" "):
                tmpData.append(float(dt))
            if idx < 38365:
                leftPoints.append(tmpData)
            elif idx < 38365 * 2:
                tmpData[0] -= 3000
                midPoints.append(tmpData.copy())
            else:
                tmpData[0] -= 6000
                rightPoint.append(tmpData)
    return leftPoints,midPoints,rightPoint
def left_face_trans(tensor_mid):
    vs_t_array = tensor_mid.detach().numpy()[0]
    vs_t_left=transform_points_to_side_view(vs_t_array, R_to_l, T_to_l)
    vs_t_right = transform_points_to_side_view(vs_t_array, R_to_r, T_to_r)
    # show_mult_face(vs_t_array,vs_t_left,vs_t_right)
    return torch.tensor([vs_t_left]).float(),torch.tensor([vs_t_right]).float()

if __name__=="__main__":


    leftPoints,midPoints,rightPoint=getpoints()
    transformed_points=np.array(midPoints)
    trans_left=np.array(leftPoints)
    trans_right=np.array(rightPoint)


    mid_tran_side=transform_points_to_side_view(transformed_points,R_to_l,T_to_l)
    # mid_tran_side=transform_points_to_side_view(transformed_points,R_to_r,T_to_r)

    mid_side = o3d.geometry.PointCloud()
    mid_side.points = o3d.utility.Vector3dVector(mid_tran_side)
    mid_side.paint_uniform_color([255, 0, 0])

    #pcd_left蓝
    pcd_left = o3d.geometry.PointCloud()
    pcd_left.points = o3d.utility.Vector3dVector(trans_left)
    pcd_left.paint_uniform_color([0, 0, 255])

    #pcd_right绿
    pcd_right = o3d.geometry.PointCloud()
    pcd_right.points = o3d.utility.Vector3dVector(trans_right)
    pcd_right.paint_uniform_color([0, 255, 0])

    #pcd_mid黄
    pcd_mid = o3d.geometry.PointCloud()
    pcd_mid.points = o3d.utility.Vector3dVector(transformed_points)
    pcd_mid.paint_uniform_color([255, 255, 0])
    o3d.visualization.draw_geometries([pcd_mid ,pcd_left,pcd_right,mid_side])